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Abstract —Recent work on simultaneous trajectory estimation 
and mapping (STEAM) for mobile robots has found success 
by representing the trajectory as a Gaussian process. Gaussian 
processes can represent a continuous-time trajectory, elegantly 
handle asynchronous and sparse measurements, and allow the 
robot to query the trajectory to recover its estimated position 
at any time of interest. A major drawback of this approach 
is that STEAM is formulated as a batch estimation problem. 
In this paper we provide the critical extensions necessary to 
transform the existing batch algorithm into an extremely efficient 
incremental algorithm. In particular, we are able to vastly speed 
up the solution time through efficient variable reordering and 
incremental sparse updates, which we believe will greatly increase 
the practicality of Gaussian process methods for robot mapping 
and localization. Einally, we demonstrate the approach and its 
advantages on both synthetic and real datasets. 

I. Introduction & Related Work 

Simultaneously recovering the location of a robot and a 
map of its environment from sensor readings is a fundamental 
challenge in robotics misiiii. 

Well-known approaches to this problem, such as square 
root smoothing and mapping (SAM) 0, have focused on 
regression-based methods that exploit the sparse structure 
of the problem to efficiently compute a solution. The main 
weakness of the original SAM algorithm was that it was a 
batch method; all of the data must be collected before a 
solution can be found. For a robot traversing an environment, 
the inability to update an estimate of its trajectory online is 
a significant drawback. In response to this weakness, Kaess 
et al. ifT^ developed a critical extension to the batch SAM 
algorithm, incremental smoothing and mapping (iSAM), that 
overcomes this problem by incrementally computing a solu¬ 
tion. The main drawback of iSAM, was that the approach 
required costly periodic batch steps for variable reordering 
to maintain sparsity and relinearization. This approach was 
extended in iSAM 2.0 lfT3l . which employs an efficient data 
structure called the Bayes tree d to perform incremental 
variable reordering and just-in-time relinearization, thereby 
eliminating the bottleneck caused by batch variable reordering 
and relinearization. The iSAM 2.0 algorithm and its extensions 
are widely considered to be state-of-the-art in robot trajectory 
estimation and mapping. 

The majority of previous approaches to trajectory estimation 
and mapping, including the smoothing-based SAM family of 


algorithms, have formulated the problem in discrete time ifTdl 
dn i n E d m. However, discrete-time representations 
are restrictive: they are not easily extended to trajectories 
with irregularly spaced waypoints or asynchronously sampled 
measurements. A continuous-time formulation of the SAM 
problem where measurements constrain the trajectory at any 
point in time, would elegantly contend with these difficulties. 
Viewed from this perspective, the robot trajectory is a function 
x{t), that maps any time f to a robot state. The problem of 
estimating this function along with landmark locations has 
been dubbed simultaneous trajectory estimation and mapping 

(STEAM) El- 

Tong et al. 1^ proposed a Gaussian process (GP) regres¬ 
sion approach to solving the STEAM problem. While their 
approach was able to accurately model and interpolate asyn¬ 
chronous data to recover a trajectory and landmark estimate, 
it suffered from significant computational challenges; naive 
Gaussian process approaches to regression have notoriously 
high space and time complexity. Additionally, Tong et al.’s 
approach is a batch method, so updating the solution ne¬ 
cessitates saving all of the data and completely resolving 
the problem. In order to combat the computational burden, 
Tong et al.’s approach was extended in Barfoot et al. 0 
to take advantage of the sparse structure inherent in the 
STEAM problem. The resulting algorithm significantly speeds 
up solution time and can be viewed as a continuous-time 
analog of Dellaert’s original square-root SAM algorithm 0. 
Unfortunately, like SAM, Barfoot et al.’s GP-based algorithm 
remains a batch algorithm, which is a disadvantage for robots 
that need to continually update the estimate of their trajectory 
and environment. 

In this work, we provide the critical extensions necessary 
to transform the existing Gaussian process-based approach 
to solving the STEAM problem into an extremely efficient 
incremental approach. Our algorithm elegantly combines the 
benefits of Gaussian processes and iSAM 2.0. Like the GP 
regression approaches to STEAM, our approach can model 
continuous trajectories, handle asynchronous measurements, 
and naturally interpolate states to speed up computation and 
reduce storage requirements, and, like iSAM 2.0, our approach 
uses a Bayes tree to efficiently calculate a maximum a poste¬ 
riori (MAP) estimate of the GP trajectory while performing 


incremental factorization, variable reordering, and just-in-time 
relinearization. The result is an online GP-based solution to 
the STEAM problem that remains computationally efficient 
while scaling up to large datasets. 

II. Batch Trajectory Estimation & Mapping as 
Gaussian Process Regression 

We begin by describing how the simultaneous trajectory 
estimation and mapping (STEAM) problem can be formulated 
in terms of Gaussian process regression. Eollowing Tong et 
al. 123 and Barfoot et al. ||2|, we represent robot trajectories 
as functions of time t sampled from a Gaussian process: 

x{t) ~ gp{n{t), K{t, t')), to < t, t' (1) 


compute the maximum a posteriori (MAP) estimate of the 
combined state conditioned on measurements via Bayes’ rule: 
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where the norms are Mahalanobis norms defined as: ||e|||; = 
and h{6) and R are the mean and covariance of the 
measurements collected, respectively: 

h{e) = [h^{ei) 112 ( 62 ) ... hN{eN)y (?) 
i? = diag(i?i,H2,...,i2Ar) ( 8 ) 


Here, x{t) is the continuous-time trajectory of the robot 
through state-space, represented by a Gaussian process with 
mean y,{t) and covariance functions. 

We next define a finite set of measurements: 

y^ = hi{e^) + rii, NiO, Ri), i = l,2,...,N (2) 

The measurement yi can be any linear or nonlinear func¬ 
tions of a set of related variables 6i plus some Gaussian noise 
rii. The related variables for a range measurement are the robot 
state at the corresponding measurement time x(ti) and the 
associated landmark location £j. We assume the total number 
of measurements is N, and the number of trajectory states at 
measurement times are M. 

Based on the definition of Gaussian processes, any finite col¬ 
lection of robot states has a joint Gaussian distribution QSl.So 
the robot states at measurement times are normally distributed 
with mean /x and covariance IC. 

x^N{y.,K), x=[x{tiy ... x{tMV y 

/x=[/x(fi)T ... V, 

Note that any point along the continuous-time trajectory can 
be estimated from the Gaussian process model. Therefore, the 
trajectory does not need to be discretized and robot trajectory 
states do not need to be evenly spaced in time, which is an 
advantage of the Gaussian process approach over discrete-time 
approaches (e.g. Dellaert’s square-root SAM |f5l). 

The landmarks £ which represent the map are assumed to 
conform to a joint Gaussian distribution with mean d and 
covariance W (Eq. |^. The prior distribution of the combined 
state 6 that consists of robot trajectory states at measurement 
times and landmarks is, therefore, a joint Gaussian distribution 
(Eq. 0. 
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To solve the STEAM problem, given the prior distribution of 
the combined state and the likelihood of measurements, we 


Because both covariance matrices R and R are positive 
definite, the objective in Eq. [^corresponds to a least squares 
problem. Consequently, if some of the measurement functions 
hi{-) are nonlinear, this becomes a nonlinear least squares 
problem, in which case iterative methods including Gauss- 
Newton and Levenberg-Marquardt i) can be utilized. A lin¬ 
earization of a measurement function at current state estimate 
9i can be accomplished by a first-order Taylor expansion: 

59, (9) 

9i 

Combining Eq. j^with Eq.|^ the optimal increment 59* at the 
current combined state estimate 9 is 
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Where H is the measurement Jacobian matrix: 

dh. 
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To solve the linear least squares problem in Eq. 10 we take 
the derivative with respect to 59, and set it to zero, which 
gives us 59* embedded in a set of linear equations 

{T~^+WR-^H)59*=V-\r}-9)^WR-^{y-h) ( 12 ) 

-V-^ -V.-^ 
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with covariance 


coy{59*,59*)=I-^ (13) 

The positive definite matrix 'P~^ -b H'^R~^H is the a 
posteriori information matrix, which we label X. To solve 
this set of linear equations for 59*, we do not actually 
have to calculate the inverse Instead, factorization-based 
methods can provide a fast, numerically stable solution. Eor 
example, 59* can be found by first performing a Cholesky 
factorization CC^ — X, and then solving Cd — b and 
59* = d by back substitution. At each iteration we perform 
a hatch state estimation update 9-^9^ 59* and repeat the 
process until convergence. 

If X is dense, the time complexity of a Cholesky factoriza¬ 
tion and back substitution are 0{n^) and 0{'n?) respectively. 












where X e ||9l. However, if X has sparse structure, 

then the solution can be found much faster. For example, for a 
narrowly banded matrix, the computation time is 0(n) instead 
of 0{n?) in. Fortunately, we can guarantee sparsity for the 
STEAM problem (see Section [Il-B| below). 

A. State Interpolation 

An advantage of the Gaussian process representation of the 
robot trajectory is that any trajectory state can be interpolated 
from other states by computing the posterior mean EOl : 

x{t) = pb{t)+K{t)K~^{x — pi), (14) 

with 


x = [ x{ti) ... x{tM) and 
}Cit) = [}C{t,h) ... ]■ (15) 


By utilizing interpolation, we can reduce the number of robot 
trajectory states that we need to estimate in the optimization 
procedure IMI . For simplicity, assume Oi, the set of the related 
variables of the zth measurement according to the model 
(Eq. 1^, is x{tj). Then, after interpolation, Eq. [^becomes; 


hi {Oi + 56i) = hi {x{tj) + 5x{tj)) 
dhi dx{tj) 


■ h^{x{ti)) + 
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dx {tj ) dx 

= h,{fi{tj)+K{tj)K~\x-pL))+H,K.{tj)K.~^6x (16) 


By employing Eq. during optimization, we can make use 
of measurement i without explicitly estimating the trajectory 
states that it relates to. We exploit this advantage to greatly 
speed up the solution to the STEAM problem in practice 
(Section |V|. 


B. Sparse Gaussian Process Regression 

The efficiency of the Gaussian Process Gauss-Newton al¬ 
gorithm presented in Section is heavily dependent on the 
choice of kernel. It is well-known that if the information matrix 
X is sparse, then it is possible to very efficiently compute the 
solution to Eq. 12 Q. Barfoot et al. suggest a kernel matrix 
with a sparse inverse that is well-suited to the simultaneous 
trajectory estimation and mapping problem in. In particular, 
Barfoot et al. show that K~^ is exactly block-tridiagonal 
when the GP is assumed to be generated by linear, time- 
varying (LTV) stochastic differential equation (SDE) which 
we describe here: 


x{t) = A[t)x{t) + v{t) -f F{t)w{t), (17) 

w{t)QV{0, QcS{t — t')) (18) 

where x{t) is trajectory, v{t) is known exogenous input, w{t) 
is process noise, and F{t) is time-varying system matrix. The 
process noise w{t) is modeled by a Gaussian process, and (5(-) 
is the Dirac delta function. (See El for details). We consider 
a specific case of this model in the experimental results in 
Section IV-AI 

Assuming the GP is generated by Eq. the measurements 
are landmark and odometry measurements, and the variables 



(a) XL ordering (b) SYMAMD ordering 

Eig. 1: Sparse information matrices. The information matrix 
X with XL ordering and SYMAMD ordering Both sparse 
matrices have the same number of non-zero elements, yet the 
second matrix can be factored much more efficiently due to 
the heuristic ordering of the matrix columns. (See Table |^. 
Eor illustration, only 200 trajectory states are shown here. 


are ordered in XL ordering the sparse information matrix 
becomes 


X = 


tt 


1-xl 

Xee 


(19) 


where X^^. is block-tridiagonal and Xu is block-diagonal. 
Xxi'& density depends on the frequency of landmark measure¬ 
ments, and how they are taken. See Eig. [T^for an example. 

When the GP is generated by LTV SDE, Barfoot et al. prove 
that 1C{t)}C~^ in Eq. 


14 


has a specific sparsity pattern, only 
two column blocks that correspond to trajectory states at ti 
and ti+i, where ti < t < ti+i, are nonzero. In other words, 
x{t) is an affine function of only two nearby states xlfi) and 

X{ti+i)\ 

x{t) =p{t) -f A(f) {x{U)-pL{ti)) + ^{t) {x{ti+i)-p,{ti+i)) , 

ti <t < ti+i ( 20 ) 


Thus, it only takes 0(1) time to query any x(t) using Eq. 20 


Moreover, because interpolation of a state is only determined 


by the two nearby states, measurement interpolation in Eq. 16 
can be significantly simplified. 


III. Batch GP-Regression with Variable 
Reordering 

Previous work on batch continuous-time trajectory estima¬ 
tion as sparse Gaussian process regression EiEl assumes 
that the information matrix X is sparse (Eq. [T^ and applies 
standard block elimination to factor and solve Eq. [T^ Despite 
the sparsity of X, for large numbers of landmarks this process 
can be very inefficient. Inspired by square root SAM 0, which 
uses variable reordering for efficient Cholesky factorization in 
a discrete-time context, we show that factorization-time can 
be dramatically improved by matrix column reordering in the 
sparse Gaussian process context as well. 


* XL ordering is an ordering where process variables come before land¬ 
marks variables. 

















TABLE I: Cost of Cholesky factorization with different order¬ 
ing methods including ordering time 



Xlfl 

SYMAMD 

Block SYMAMD 

nnz 

r 

1817499 

192285 

176105 

time 

(sec) 

0.967720 

0.027402 

0.017500 


It is reasonable to base our approach on SAM because the 
information matrix and factor graph of the sparse GP ||2| 
has structure similar to the SAM formulations of the prob¬ 
lem m El, and the intuitions from previous discrete-time 
approaches apply here. If the Cholesky decompositions are 
performed naively, fill-in can occur, where entries that are zero 
in the information matrix become non-zero in the Cholesky 
factor. This occurs because the Cholesky factor of a sparse 
matrix is guaranteed to be sparse for some variable orderings, 
but not all variable orderings ini. Therefore, we want to hnd 
a good variable ordering so that the Cholesky factor is sparse. 

Although hnding the optimal ordering for a symmetric 
positive dehnite matrix is NP-complete ETI . good heuristics 
do exist. One such heuristic is symmetric approximate min¬ 
imum degree permutation (SYMAMD^ ID. To demonstrate 
the benefits of variable reordering, we constructed a synthetic 
example and compared different approaches. The example, 
which is explained in detail in Section V-A[ consists of 1,500 
time steps with trajectory states, x{ti) = [ p{ti) p{ti) ]''', 
p{ti) = [ x{ti) y{ti) 9{ti) ]''', and with odometry and 
range measurements. The total number of landmarks is 298. 
The structure of the information matrix X and Cholesky factor 
C, with and without variable reordering, are compared in 
Fig. 0 and Fig. Although variable reordering does not 
change the sparsity of the information matrix I (Fig. 0’ it 
dramatically increases the sparsity of the Cholesky factor C 
(Fig.0. Table demonstrates this clear beneht of reordering. 
The Cholesky factor after SYMAMD ordering contains 10.6% 
non-zeroes of XL ordering and takes 2.83% of the time, 
which are signihcant improvements in both time and space 
complexity. 

We also experimented with block SYMAMD la, which 
exploits domain knowledge to group together variables be¬ 
longing to a particular trajectory state x{ti) or landmark 
location before performing SYMAMD and empirically 
further improves performance. 

It is straightforward to incorporate variable reordering meth¬ 
ods like SYMAMD and block SYMAMD into the batch GP- 
Regression algorithm from Section Given a new batch of 
data, directly update the sparse information matrix X, reorder 
the variables with (block) SYMAMD, and then recompute the 


Cholesky factor C on the way to solving for 50 in Eq. 12 


In most STEAM problems, we are interested in estimating 
the robot’s trajectory as it traverses the environment. In 
Alg. B we accomplish this by repeatedly executing the batch 
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Fig. 2: The Cholesky factors C of X. In (a), C is computed 
with XL ordering which exhibits hll-in. When computed with 
SYMAMD ordering in (b), C is more sparse. For illustration, 
only 200 states are shown here. 


algorithm with variable reordering. Although this approach 
seems like it should be very costly, with variable reordering 
this method it is actually quite efficient. Building and fac¬ 
toring the sparse information matrix is much faster than the 
linearization step required for a single iteration of the Gauss- 
Newton algorithm. Since the computational bottleneck is not 
the Cholesky decomposition, but rather the relinearization of 
the measurement model, we suggest only periodic Gauss- 
Newton iterations. 


Algorithm 1 Periodic Batch Sparse GP Regression 

while collecting data do 

1. Get measurement results belonging to this period, y [y, ynewY 

2. Initial guess for the newly encountered states, 6 ^ [0, OnewY 

3. Build the measurement Jacobian H, and then X and b required in 

Eq-P 

4. Find an ordering p for X, and reorder Xp X, bp b 

5. Solve XpSOp = bp using Cholesky factorization 

6. Recover the solution S9* 4^ 50* by inverse ordering r = p~^ 

1. Update estimate 0 <— 0 -|- 50* 

end while 


IV. Bayes Tree for Fast Incremental Updates to 
Sparse GP Regression 

Despite the efficiency of periodic batch updates, Alg. [T] 
is still repeatedly executing a batch algorithm that requires 
reordering and refactoring X, and periodically relinearizing 
the measurement function for all of the estimated states each 
time new data is collected. Here we provide the extensions 
necessary to avoid these costly steps and turn the naive batch 
algorithm into an efficient, truly incremental, algorithm. The 
key idea is to perform just-in-time relinearization and to 
efficiently update an existing sparse factorization instead of 
re-calculating one from scratch. 


^SYMAMD is a variant of column approximate minimum degree ordering 
(COLAMD) |4) on positive definite matrix. 


^The number of non-zero elements. 




















Fig. 3; A simple factor graph that includes landmark measure¬ 
ments, odometry measurements, and Gaussian process priors. 


A. The Bayes Tree Data Structure 

We base our approach on iSAM 2.0 proposed by Kaess et 
al. El, which was designed to efficiently solve a nonlinear 
estimation problem in an incremental and real-time manner 
by directly operating on the factor graph representation of the 
SAM problem. The core technology behind iSAM 2.0 is the 
Bayes tree data structure which allows for incremental vari¬ 
able reordering and fluid relinearization m. Demonstrated 
by Kaess et al. El, Bayes tree provides dramatic speedup 
compared to batch method, with negligible loss in accuracy. 
We apply the same data structure to sparse Gaussian process 
regression in the context of the STEAM problem, thereby 
eliminating the need for periodic batch computation. 

To understand how the Bayes tree is used, it is helpful to un¬ 
derstand how the GP estimation problem can be represented as 
a factor graph Qa . Formally, a factor graph is a bipartite graph 
G = 9,8), where F is the set of factor nodes that encodes 

all probabilistic constraints on variables, including landmark 
measurements, odometry measurements, and smoothing priors, 
0 is the set of variable nodes to estimate, and 8 is the set of 
edges that connect factors nodes with variable nodes. The joint 
probability of variables to estimate is factored as 

f{o) = llLm ( 21 ) 

i 


where G is one of the factors, and 6i is the set of variables 
directly connected to fi. The estimation problem is to find 6* 
that maximizes Eq. As stated in Section II-B[ Barfoot et 
al. prove that when GPs are generated by LTV SDE, K.~^ is 
block-tridiagonal ||2l. They also state that the factors resulted 
from the Gaussian process representation of the trajectory, or 
the Gaussian process prior factors, only connect consecutive 
pairs of states. This leads to a sparse Eactor graph (Eig. [^. 

The factor graph can be converted to a Bayes net by a 
bipartite elimination game in]. The procedure is equivalent 
to converting Eq. to least squares form and computing the 
square-root information matrix C via an incomplete Cholesky 
factorization. To facilitate marginalization and optimization, 
a Bayes tree, which groups several variables together based 
on their dependence, is constructed from the Bayes net m. 
Erom a linear algebra perspective, the Bayes tree captures the 
structure of the Cholesky factor C of X, and the sequence of 
back substitutions that can be performed. 

When we add a new measurement, add a prior for a new 


variable, or relinearize a previous measurement, C will change 
accordingly. However, all modifications to the factor graph 
only have local effects on C. Exploiting this observation is the 
foundation for efficient incremental updates. Since the nodes 
of Bayes tree encode conditional probability distributions 
which directly correspond to rows in C, the structure of the 
tree can be leveraged to efficiently update the factor c na. 

The nodes containing variables involved in new factors, 
or nodes Oun whose linear step is larger than a predetermined 
threshold, are identified. 

Only these nodes and their ascendants in the Bayes tree 
are then updated. When a sub-tree is updated, variables in 
the sub-tree are reordered by constrained COLAMD H to 
ensure sparsity and heuristically maximize the locality of 
future updates. Einally, 56* is computed from tree root to 
leaves. Propagation stops when updates to the conditioning 
variables are below a predetermined threshold. 

Algorithm summarizes incremental Gaussian process re¬ 
gression with the Bayes tree data structure in detail. The 
algorithm also incorporates state interpolation, described in 
the next section. 


B. Faster Updates Through Interpolation 

To further reduce computation time, we take advantage of 
Gaussian process state interpolation (as suggested by Tong 
et al. EOll ') within our incremental algorithm. This allows us to 
reduce the total number of estimated states, while still using all 
of the measurements, including those that involve interpolated 
states. By only estimating a small fraction of the states along 
the trajectory, we realize a significant speedup relative to a 
naive application of the Bayes tree (see Section |V]i. This is an 
advantage of GP-based methods with respect to discrete-time 
methods like iSAM 2.0. 

Algorithm 1^ describes how interpolation is used within our 
incremental algorithm: Eirst, when a measurement related to a 
missing state is received, the variables necessary to interpolate 
the state, as well as the corresponding cliques in Bayes tree 
that should be removed or updated, are identified. Since the 
sparse GP has a LTV SDE prior, each interpolated state is only 
a function of two nearby states (see Eq. [20| i. These nearby 
states are therefore included into the set of variables 6nf 
related to the new factor (line 1). In the case that the GP 
relies on a different kernel matrix, the corresponding states 
used for interpolation can be determined from Eq. Second, 
linearization of factors that involve missing states (line 3) is 


performed by incorporating state interpolation via Eq. 16 


V. Experimental Results 


We evaluate the performance of our incremental sparse 
GP regression algorithm to solving the STEAM problem on 
synthetic and real-data experiments and compare our approach 
to the state-of-the-art. In particular, we evaluate how variable 
reordering can dramatically speed up the batch solution to the 
sparse GP regression problem, and how, by utilizing the Bayes 


"^The reader is referred to (m for additional details regarding this just-in- 
time relinearization. 







Algorithm 2 Updating Sparse GP Regression by Bayes Tree 

while collecting data do 

1. Get new measurement results, store new factors Tnew and identify 
related variables Onf = \jOi, fi ^ J^new- If the state x{ti) G O nf i s 
missing, then it is replaced by variables used in interpolation (Eg. |2Q) ; 
If x{ti) G Onf is a new state to estimate, the previous state to estimate 
is added to Onf, and a Gaussian process prior factor is stored. 

2. For each affected variable in Oaff = Oun U Onf, remove the 
corresponding clique and ascendants up to the root of Bayes tree. 

3. Relinearize the factors required to recreate the removed part. Use 
interpolation when linearizing factors involving missing states (Eq. |16t 

4. Add cached marginal factors from orphaned sub-trees of removed 
cliques and create a factor graph 

5. Eliminate the factor graph by a new variable ordering, create a Bayes 
tree, and attach back orphaned sub-trees 

6. Partially update estimate from root to leaves and stop walking down a 
branch when the updates to variables that the child clique is conditioned 
on are not significant enough 

7. Collect variables involved in the measurement factors J^nn where 
previous linearization point is fai‘ from current estimate, O^n = (jOi, 
fi C J'lin 

end while 



Fig. 4; Synthetic dataset; Ground truth and state estimates 
are shown. Lines between trajectory points and landmarks 
indicate range measurements. State estimates obtained from 
BTGP approach are very close to ground truth. 


tree and interpolation for incremental updates, our algorithm 
can yield even greater gains in the online trajectory estimation 
scenario. We compare; 


PB; Periodic batch (described in Section E- This is the 
state-of-the-art algorithm presented in Barfoot et al. 11 
(XL variable ordering), which is periodically executed as 
data is received. 

PBVR: Periodic batch with variable reordering (described 
in Sectioning. 

BTGP: The proposed approach - Bayes tree with Gaus¬ 
sian process prior factors (described in Section IV i. 


If the GP is only used to estimate the state at measurement 
times, the proposed approach offers little beyond a reinter¬ 
pretation of the standard discrete-time iSAM 2.0 algorithm. 
Therefore, we also compare our GP-based algorithm, which 
leverages interpolation, to the standard Bayes tree approach 
used in iSAM 2.0. We show that by interpolating large 
fractions of the trajectory during optimization, the GP allows 
us to realize significant performance gains over iSAM 2.0 with 
minimal loss in accuracy. For these experiments we compare: 


• without interpolation: BTGP without interpolation at 
a series of lower temporal resolutions. Without interpo¬ 
lation BTGP is algorithmically identical to ISAM 2.0. 
Measurements between two estimated states are simply 
ignored. 

• with interpolation: BTGP with interpolation at a series 
of lower resolutions. In contrast to the above case, 
measurements between estimated states are fully utilized 
by interpolating missing states at measurement times 
(described in Section |IV-B| l. 

• finest estimate: The baseline. BTGP at the finest reso¬ 
lution, estimating all states at measurement times. When 
measurements are synchronous with evenly-spaced way- 
points and no interpolation is used, BTGP is identical to 
ISAM 2.0 applied to the full dataset. 


All algorithms are implemented with the same C-n- libray, 
GTS AM 3.2j^to make the comparison fair and meaningful. 
Evaluation is performed on three datasets summarized in 
Table |I^ We first evaluate performance in a synthetic dataset 
(Section |V-A| l, analyzing estimation errors with respect to 
ground truth data. Results using real-world datasets are then 


presented in Sections V-B and V-C 


A. Synthetic SLAM Exploration Task 

This dataset consists of an exploration task with 1,500 time 
steps. Each time step contains a trajectory state xiti) — 
[ p(ti) p{ti) ]T, p{ti) = [ x{ti) y{ti) 9{ti) Y, an 

odometry measurement, and a range measurement related to 
a nearby landmark. The total number of landmarks is 298. 
The trajectory is randomly sampled from a Gaussian process 
generated from white noise acceleration p{t) = w{t), i.e. 
constant velocity, and with zero mean. 


x{t) = Ax[t) + Fw{t) (22) 


where 

x{t) = 

F = 


Pit) 

pit) 


1 

'x{t) 


0 I 

II 

' ^ 

|^+- c-t- 

1 _ 

II 

0 0 


w(t) gv{o, QcSit -1')) 


(23) 


Note that velocity p{t) must to be included in trajectory state 
to represent the motion in LTV SDE form ||2l. 

The odometry and range measurements with Gaussian noise 
are specified in Eq. and Eq. respectively. 


Vio 


cos 9{u) ■ x{ti) + sin 9{ti) ■ y{U) 

Hu) 


+ rio 


(24) 


yir=\\[xiti) yiti)y - + (25) 


^https://collab.cc.gatech.edu/borg/gtsam/ 


























TABLE II: Summary of experimental datasets 



# time steps 

# odo. m. 

# landmark m. 

# landmarks 

travel dist.(km) 

Synthetic 

1,500 

1,500 

1,500 

298 

0.2 

Auto. Mower 

9,658 

9,658 

3,529 

4 

1.9 

Victoria Park 

6,969 

6,969 

3,640 

151 

3.5 



Fig. 6: The Autonomous Lawnmower dataset: Ground truth 
and state estimates are shown. The range measurements are 
sparse, noisy, and asynchronous. Ground truth and state esti¬ 
mates obtained from BTGP are very close. 


where yio consists of the robot-oriented velocity and heading 
angle velocity with Gaussian noise, and yir is the distance 
between the robot and a specihc landmark at L with 
Gaussian noise. 

We compare the computation time of the three approaches 
(PB, PBVR and BTGP) in Fig. The incremental Gaussian 
process regression (BTGP) offers significant improvements in 
computation time compared to the batch approaches (PBVR 
and PB). 

We also demonstrate that BTGP can further increase speed 
over a naive application of the Bayes tree (e.g. iSAM 2.0) 
without sacrificing much accuracy by leveraging interpolation. 
To illustrate the trade-off between the accuracy and time 
efficiency due to interpolation, we plot RMSE of distance 
erTors and the total computation time by varying the time step 
difference (the rate of interpolation) between estimated states. 

B. The Autonomous Lawnmower 

The second experiment evaluates our approach on real data 
from a freely available range-only SLAM dataset collected 
from an autonomous lawn-mowing robot ||7]. The “Plaza” 
dataset consists of odometer data and range data to stationary 
landmarks collected via time-of-flight radio nodes. (Additional 
details on the experimental setup can be found in Q.) Ground 
truth paths are computed from GPS readings and have 2cm 
accuracy according to Q. 

The environment, including the locations of the landmarks 
and the ground truth paths, are shown in Fig. The robot 
travelled L9km, occupied 9,658 poses, and received 3,529 


range measurements, while following a typical path generated 
during mowing. The dataset has sparse range measurements, 
but contains odometry measurements at each time step. The 
results of incremental BTGP are shown in Fig. and demon¬ 
strate that we are able to estimate the robot’s trajectory and 
map with a very high degree of accuracy. 

As in Section V-A[ performance of three approaches - peri¬ 
odic batch relinearization (PB), periodic batch relinearization 
with variable reordering (PBVR) and incremental Bayes tree 
(BTGP) are compared in Fig. 

In this dataset, the number of landmarks is 4, which is 
extremely small relative to the number of trajectory states, 
so there is no performance gain from reordering. However, 
the Bayes tree-based approach dramatically outperforms the 
other two approaches. As the problem size increases, there 
is negligible increase in computation time, even for close to 
10,000 trajectory states. 


C. Victoria Park 


The third experiment evaluates our approach on the Victoria 
Park dataset Qa, which consists of range-bearing measure¬ 
ments to landmarks, and speed and steering odometry mea¬ 
surements. The data was collected from a vehicle equipped 
with a laser sensor driving through the Sydney’s Victoria Park. 
The environment contains a high number of trees as land¬ 
marks. The vehicle travelled ~ 3.5 km in 26 minutes. After 
repeated measurements, taken when the vehicle is stationary, 
are dropped, the dataset consists of 6,969 time steps and 3,640 
range-bearing measurements relative to 151 landmarks. The 


bearing measurement is specihed in Eq. 26 as the relative 
angle from vehicle heading to the landmark direction with 
Gaussian noise where [ Xj yj is location of landmark j. 


yib = atan2 {yj - y{ti), Xj - x{ti)) - 9{ti) + (26) 

The results, shown in Figure further demonstrate the 
advantages of BTGP. As seen from the upper right plot, 
variable reordering drastically reduces computation time when 
used within batch optimization (PBVR), and even further in 
the incremental algorithm (BTGP). 


VI. Conclusion 

We have introduced an incremental sparse Gaussian pro¬ 
cess regression algorithm for computing the solution to the 
continuous-time simultaneous trajectory estimation and map¬ 
ping (STEAM) problem. The proposed algorithm elegantly 
combines the benefits of Gaussian process-based approaches 
to STEAM while simultaneously employing state-of-the-art 
innovations from incremental discrete-time algorithms for 






















time step time step difference between two estimated states 




Fig. 5; Synthetic dataset: (Left Column) Comparison of the computation time of three approaches PB, PBVR, and BTGP 
The modifiers /I and /lO indicate frequency of state updates. For example: BTGP/1 updates the estimate after 1 new range 
measurement using BTGP. Likewise BTGP/10 updates the estimate after 10 new range measurements using BTGP. For fair 
comparison no interpolation is used by BTGP. Due to the large number of landmarks, 298, compared to the number of trajectory 
states, variable reordering dramatically improves the performance. (Right Column) Trade-off between computation time and 
accuracy if BTGP makes use of interpolation. The y-axis measures the RMSE of distance errors of the estimated trajectory states 
and total computation time with increasing amounts of interpolation. The x-axis measures the time step difference between 
two estimated (non-interpolated) states. “Without interpolation” means that the number of states is reduced, but measurements 
taken at the missing states are ignored. This is equivalent to running iSAM 2.0 to hnd a trajectory with coarse discretization. 
“With interpolation” is the BTGP algorithm that interpolates missing states while incorporating odometry measurements at the 
interpolated states. “Finest estimate” is the baseline which measures RMSE and computation time if the number of states is 
not reduced. This is exactly equivalent to ISAM 2.0 run on the full measurement and odometry data. The results indicate that 
interpolating ~ 90% of the states (i.e. estimating only ~ 10% of the states) while running BTGP can result in a 33% reduction 
in computation time over iSAM 2.0 without sacrificing accuracy. 


smoothing and mapping. Our empirical results show that by 
parameterizing trajectories with a small number of states and 
utilizing Gaussian process interpolation, our algorithm can 
realize large gains in speed over iSAM 2.0 with very little 
loss in accuracy (e.g. reducing computation time by 68% 
while increasing RMSE by only 8cm on the Autonomous 
Lawnmower Dataset) . 
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Fig. 8; Victoria Park dataset; (left) dead reckoning and estimated path obtained from BTGP approach, (right) Comparison of 
the computation time of three approaches PB, PBVR, and BTGP. As in Figures and the modifiers /I and /lO indicate 
frequency of state updates. Updates are very fast (close to zero time) when there are no range measurements. Since many 
landmarks are involved, PBVR dramatically improves performance, compared PB. The incremental BTGP algorithm improves 
performance even further. Unlike in previous datasets, we did not evaluate the trade-off between interpolation and accuracy for 
Victoria Park, since we do not have access to ground truth and cannot evaluate the effect on accuracy. However, like previous 
datasets, interpolation can greatly increase the speed of BTGP. 
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